#ifndef MPC_EW2D_PatternedW_H
#define MPC_EW2D_PatternedW_H

#include "Controller.h"
#include "equations.h"
// -------------------------------------------
// class: MPC1_PatternedW
// == MV ==
// MV[0] = T (Fixed value)
// MV[1] = W
// MV[2] = A
// MV[3] = Freq (Fixed value)
//
// == Parameters ==
// Para[0] = r2_set
// Para[1] = q_r2
// Para[2] = m2_set
// Para[3] = q_m2
// Para[4] = T
// Para[5] = Freq
//
// == Optimization problem ==
// min_{W_0,A}{J(T,W0,A)}
// where 
// >> J = q_r\times(r^2_f-r^2_sp)^2+q_m\times(m^2_f-m^2_sp)^2
//    r^2_f is the predicted value of r^2 at t = t_end
// >> r^2_f = eq_r2(W0,A,t_end-t_now,current_surface)
//    m^2_f is defined in a similar way
// >> m^2_f = eq_m2(W0,A,t_end-t_now,current_surface)
// S.T.:
//   W_min <= W <= W_max
//   \abs{W(t)-W(t_dt)}<= dW_max
//   0 < A < W
//
// SUMMARY
// Parameters: 
//  - From feedback: current_surface, t_end-t_now
//  - Tuning parameters: r^2_sp, m^2_sp, q_r, q_m
// Decision variables: W0, A
// Inequality constraints: 
// -\infty <= g(x) = A-W = x[2]-x[1] <= 0
// -- Upper/lower limits of variables --
// max{W_{min},W_old_dW_max} <= W <= min{W_{max},W_old+dW_max}
// 0 <= A <= \infty
//
// -----------------------------------------

class MPC1_PatternedW : public MPC_Formulation{
  // parameters in the optimization problem
  // I/O parameters -----------------
  int num_decision_val;
  double W_old;        // nm/s
  double A_old;        // nm/s
  double W_ini;        // nm/s
  double A_ini;        // nm/s


  // Cost function ------------------
  eq_f*   cost_fun;

  // Constraints -------------------
  double W_min, W_max; // Unit: nm/s
  double A_min, A_max; // Unit: nm/s
  double dW_max;       // Unit: nm/s^2
  
  // Model parameter ---------------
  double r2_set,q_r; // Unit of r2: (nm)^2
  double m2_set,q_m; // Unit of m2: dimensionless
  int    LatticeSize;  
  double XYmax;       // length of the 2D surface domain, unit: nm
  int    mode;
  double Freq;
  double T;            // K

  double t_end;       // 
  double C2;          // Unit: nm^2/s
  double sigam2;      // Unit: nm^2/s^2
  double time;        // 
  double dt;          // dt = t_end-time;

  double *cov_z_m[4]; // measurement
  double *mean_z_m[4];// 
  double *cov_z_p[4]; // prediction
  double *mean_z_p[4];
  double *dz_square_dT[4];
  double *dz_square_dW[4];
  double *dz_square_dA[4];

 public:
  /** default constructor */
  MPC1_PatternedW(int i_mode, int i_LatticeSize, 
		  double i_XYmax, double i_Freq, double i_dt,
		  double i_r2_set,double i_q_r,
		  double i_m2_set,double i_q_m);
  MPC1_PatternedW(string option_file);
  /** default destructor */
  ~MPC1_PatternedW();
  
  // Virtual function from MPC_Formulation
  virtual void reset();
  virtual void parameterize(double time,double** Inputs);
  virtual void get_solution(double* x);
  virtual void setPara(int para_id,double para_val);

  /**@name Overloaded from TNLP */
  //@{
  /** Method to return some info about the nlp */
  virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
                            Index& nnz_h_lag, IndexStyleEnum& index_style);

  /** Method to return the bounds for my problem */
  virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u,
                               Index m, Number* g_l, Number* g_u);

  /** Method to return the starting point for the algorithm */
  virtual bool get_starting_point(Index n, bool init_x, Number* x,
                                  bool init_z, Number* z_L, Number* z_U,
                                  Index m, bool init_lambda,
                                  Number* lambda);

  /** Method to return the objective value */
  virtual bool eval_f(Index n, const Number* x, bool new_x, Number& obj_value);

  /** Method to return the gradient of the objective */
  virtual bool eval_grad_f(Index n, const Number* x, bool new_x, Number* grad_f);

  /** Method to return the constraint residuals */
  virtual bool eval_g(Index n, const Number* x, bool new_x, Index m, Number* g);

  /** Method to return:
   *   1) The structure of the jacobian (if "values" is NULL)
   *   2) The values of the jacobian (if "values" is not NULL)
   */
  virtual bool eval_jac_g(Index n, const Number* x, bool new_x,
                          Index m, Index nele_jac, Index* iRow, Index *jCol,
                          Number* values);

  /** Method to return:
   *   1) The structure of the hessian of the lagrangian (if "values" is NULL)
   *   2) The values of the hessian of the lagrangian (if "values" is not NULL)
   */
  virtual bool eval_h(Index n, const Number* x, bool new_x,
                      Number obj_factor, Index m, const Number* lambda,
                      bool new_lambda, Index nele_hess, Index* iRow,
                      Index* jCol, Number* values);

  //@}

  /** @name Solution Methods */
  //@{
  /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
  virtual void finalize_solution(SolverReturn status,
                                 Index n, const Number* x, const Number* z_L, const Number* z_U,
                                 Index m, const Number* g, const Number* lambda,
                                 Number obj_value,
				 const IpoptData* ip_data,
				 IpoptCalculatedQuantities* ip_cq);
  //@}

private:
  /**@name Methods to block default compiler methods.
   * The compiler automatically generates the following three methods.
   *  Since the default compiler implementation is generally not what
   *  you want (for all but the most simple classes), we usually 
   *  put the declarations of these methods in the private section
   *  and never implement them. This prevents the compiler from
   *  implementing an incorrect "default" behavior without us
   *  knowing. (See Scott Meyers book, "Effective C++")
   *  
   */
  //@{
  //  MPC_Formulation();
  MPC1_PatternedW(const MPC_Formulation&);
  MPC1_PatternedW& operator=(const MPC_Formulation&);
  //@}
  inline int    index2D(int m,int n){return m*(mode+1)+n;};
};

#endif
